import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32MultiArray
from geometry_msgs.msg import Twist
from std_msgs.msg import Int32
from rcl_interfaces.msg import SetParametersResult
from ai_msgs.msg import PerceptionTargets
from origincar_msg.msg import Sign  # 根据图片的消息类型
import math
import datetime
import time

class PIDController:
    def __init__(self, Kp, Kd, Ki, integral_max, setpoint=0):

        self.Kp=Kp 
        self.Ki=Ki
        self.Kd=Kd 

        self.integral_max = integral_max
        self.integral_min = integral_max * (-1)
        self.setpoint = setpoint
        self.previous_error = 0
        self.integral = 0 
        
        


    def update(self, current_value, dt):
        """
        更新PID控制器的输出。

        根据当前值和设定点计算误差，并使用PID控制律更新控制器的输出。
        
        参数:
        current_value -- 当前过程变量的值
        dt -- 时间间隔，用于计算积分和微分项
        
        返回:
        控制器的输出，根据当前误差、积分误差和微分误差计算得出。
        """
        # 计算当前误差
        error = self.setpoint - current_value
        
        # 更新比例项
        proportional = self.Kp * error

        # 更新积分项
        self.integral += error * dt
        
        # 限制积分项的大小
        if self.integral > self.integral_max:
            self.integral = self.integral_max
        elif self.integral < self.integral_min:
            self.integral = self.integral_min
        
        # 计算微分项
        derivative = (error - self.previous_error) / dt
        
        # 计算PID控制律的输出
        output = proportional + self.Ki * self.integral + self.Kd * derivative
        
        # 保存当前误差，供下一次计算微分项使用
        self.previous_error = error
        
        # 返回控制器的输出
        return output

class CoordinateSubscriber(Node):
    def __init__(self):
        super().__init__('coordinate_subscriber')
        '''
        self.subscription = self.create_subscription(
            Int32MultiArray,
            '/xy_coordinates',
            self.action_track,
            10)
        #创建订阅者，订阅话题 /xy_coordinates 话题
        '''
        # 创建一个订阅者，订阅 /sigh4return 话题，消息类型为 Int32,上位机信号
        self.sign4return = self.create_subscription(
            Int32,
            '/sign4return',
            self.listener_callback_sign4return,
            10)

        #二维码识别（识别到后就暂停）
        self.qrcode_subscriber = self.create_subscription(
            Sign,
            '/sign_switch',
            self.listener_callback_qrcode_subscriber,
            10)
       #创建订阅者，订阅话题 hobot_dnn_detection 话题   
        self.subscription_yolo = self.create_subscription( 
            PerceptionTargets, 
            'hobot_dnn_detection',  
            self.listener_yolo_callback, 
            10) 
    
        self.publisher = self.create_publisher(Twist, '/cmd_vel', 10)  # 创建 Twist 消息的发布者
        self.last_time = self.get_clock().now()

        # 0：正常寻迹    1：有障碍物   2：停下   3：识别二维码  4. park
        self.state = 2     #创建状态机
        self.flag = 1
        self.center = 320,240  
        self.image_center = 320,240 
      

        #编写参数节点来实现节点的改变
        #申明接受参数
        self.declare_parameter ('kp',0.01) 
        self.declare_parameter ('ki',0) 
        self.declare_parameter ('kd',0) 
        self.declare_parameter ('vx',0.6)
        self.declare_parameter ('zhuitongyuzhi',120) 
        self.declare_parameter ('qrcodeyuzhi',50) 
        self.declare_parameter ('parkyuzhi',300)
        self.declare_parameter ('turn_time_1',1.2)
        self.declare_parameter ('turn_time_2',0.8)
        self.declare_parameter ('turn_v',0.5)
        self.declare_parameter ('turn_w',0.01)
        self.declare_parameter ('turn_error',0)
        

        #self.declare_parameter ('z' ,0)
        #编写参数
        self.kp = self.get_parameter("kp").value
        self.ki = self.get_parameter("ki").value
        self.kd = self.get_parameter("kd").value
        self.vx = self.get_parameter("vx").value
        self.zhuitongyuzhi = self.get_parameter("zhuitongyuzhi").value
        self.qrcodeyuzhi = self.get_parameter("qrcodeyuzhi").value
        self.parkyuzhi = self.get_parameter("parkyuzhi").value
        self.turn_time_1 = self.get_parameter("turn_time_1").value
        self.turn_time_2 = self.get_parameter("turn_time_2").value
        self.turn_v = self.get_parameter("turn_v").value
        self.turn_w = self.get_parameter("turn_w").value   
        self.turn_error = self.get_parameter("turn_error").value   


        #self.z  = self.get_parameter("z").value
        #参数回调函数
        self.add_on_set_parameters_callback(self.parameters_callback)
        self.pid_controller = PIDController(Kp=self.kp,Ki=self.ki,Kd=self.kd,integral_max=100,setpoint=320)  # PID 控制器

    def listener_callback_sign4return(self,msg):
        if msg.data == 6:
            self.flag = 0
            self.state = 0
            self.get_logger().info(f"结束遥感")
        if msg.data == -1:
            self.flag = 0
            self.state = 0
            self.get_logger().info(f"重置里程计")



    def listener_callback_qrcode_subscriber(self, msg):        
        if msg.sign_data in [3, 4] and self.flag == 0:
            self.state = 2  # 切换到暂停状态
            self.flag  = 1   #flag

            twist_msg = Twist()
            twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
            twist_msg.angular.z = 0.00  # 使用 角速度复位
            
            # 发布 Twist 消息
            self.publisher.publish(twist_msg)
            self.get_logger().info(f"已复位")


    def listener_yolo_callback(self, msg):
        if self.flag == 0:
            for num, target in enumerate(msg.targets):
                class_type = target.rois[0].type.strip()
                if target.rois:

                    # 获取第一个 ROI
                    roi = target.rois[0].rect

                    # 获取框参数
                    x_offset = roi.x_offset
                    y_offset = roi.y_offset
                    height = roi.height
                    width = roi.width
                    #confidence =target.rois[0].confidence
                    # 左上角、右上角、左下角、右下角的坐标
                    '''
                    top_left = (x_offset, y_offset)
                    top_right = (x_offset + width, y_offset)
                    bottom_left = (x_offset, y_offset + height)
                    bottom_right = (x_offset + width, y_offset + height)
                    '''

                    # 中心坐标
                    center = (int(x_offset + width / 2), int(y_offset + height / 2))



                if class_type == 'zhuitong' :
                    if  height >= self.zhuitongyuzhi :
                        self.state = 1
                        self.center = center
                        self.left = int(x_offset - width / 2) - self.turn_error   #校正左转点
                        self.right = int(x_offset + 3 * width /2) + self.turn_error   #校正右转点
                        self.get_logger().info(f"锥桶避障执行中")
                        self.get_logger().info(f"{self.center[0]}")  
                        
                        self.control()


                elif class_type == 'qrcode' :
                    if height >= self.qrcodeyuzhi :
                        self.state = 3
                        
                        #将x限制在100到500,避免转弯过大
                        if center[0] >= 500:
                            center_list = list(center)
                            center_list[0] = 500
                            center = tuple(center_list)
                        if center[0] <= 100:
                            center_list = list(center)
                            center_list[0] = 100
                            center = tuple(center_list)
                        
                        self.center = center
                        self.get_logger().info(f"识别到二维码")
                        self.get_logger().info(f"{self.center[0]}")  
                        self.control()

                    
                elif class_type == 'p' :
                    distance = math.sqrt((320 - center[0])**2 + (480 - center[1])**2) 
                    if distance < self.parkyuzhi :
                        self.state = 4

                        #将x限制在100到500,避免转弯过大
                        if center[0] >= 500:
                            center_list = list(center)
                            center_list[0] = 500
                            center = tuple(center_list)
                        if center[0] <= 100:
                            center_list = list(center)
                            center_list[0] = 100
                            center = tuple(center_list)

                        self.center = center
                        self.get_logger().info(f"识别到停车场")  
                        self.get_logger().info(f"{self.center[0]}")                        
                        self.control()
        
                    
                elif  class_type == 'track' :
                    if center[1] >= 240:
                        self.state = 0        

                        #将x限制在100到500,避免转弯过大
                        if center[0] >= 500:
                            center_list = list(center)
                            center_list[0] = 500
                            center = tuple(center_list)
                        if center[0] <= 100:
                            center_list = list(center)
                            center_list[0] = 100
                            center = tuple(center_list)
                            
                        self.center = center
                        self.get_logger().info(f"循迹中")  
                        self.get_logger().info(f"{self.center[0]}")                         
                        self.control()
                
                else  :
                    self.state = 0
                    self.center = self.image_center
                    self.get_logger().info(f"啥也看不见") 
                    self.get_logger().info(f"{self.center[0]}")       
                    self.control()
           



    # 0：正常寻迹    1：有障碍物   2：停下   3：识别二维码  4. park
             
    def control(self):
        if self.state == 0 :
            self.action_track()
        elif self.state == 1 :
            self.action_cone()
        elif self.state ==3 :
            self.qrcode()
        elif self.state == 4:
            self.park()

    def action_cone(self):
        twist_msg = Twist()
        if self.center[0] >= 320:
            if (320-self.left ) > 0:
                twist_msg.angular.z = self.turn_w * (320-self.left)
                twist_msg.linear.x = self.turn_v
                self.get_logger().info(f"{self.left}")
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.publisher.publish(twist_msg) 
                time.sleep(self.turn_time_1)     
                twist_msg.angular.z = - self.turn_w * (320-self.left)
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.publisher.publish(twist_msg) 
                time.sleep(self.turn_time_2)     
                return                  
            else :
                return
        if self.center[0] < 320:
            if (320-self.right ) < 0:
                twist_msg.angular.z = self.turn_w * (320-self.right)
                twist_msg.linear.x = self.turn_v
                self.get_logger().info(f"{self.right}")
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.publisher.publish(twist_msg) 
                time.sleep(self.turn_time_1)     
                twist_msg.angular.z = - self.turn_w * (320-self.right)
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.publisher.publish(twist_msg) 
                time.sleep(self.turn_time_2)
                return
            else :
                return       

    def qrcode(self):
        # 计算时间差
        current_time = self.get_clock().now()
        dt = (current_time - self.last_time).nanoseconds / 1e9
        self.last_time = current_time

        # 使用 PID 控制器计算角速度
        angular_z = self.pid_controller.update(self.center[0], dt)
        twist_msg = Twist()
        twist_msg.linear.x = self.vx     
        twist_msg.angular.z = angular_z  
        self.publisher.publish(twist_msg)

    def park(self):
        
        # 计算时间差
        current_time = self.get_clock().now()
        dt = (current_time - self.last_time).nanoseconds / 1e9
        self.last_time = current_time

        # 使用 PID 控制器计算角速度
        angular_z = self.pid_controller.update(self.center[0], dt)
        twist_msg = Twist()
        twist_msg.linear.x = self.vx     
        twist_msg.angular.z = angular_z  
        self.publisher.publish(twist_msg)



    def action_track(self):
        
        if(self.state == 0) :
            # 计算时间差
            current_time = self.get_clock().now()
            dt = (current_time - self.last_time).nanoseconds / 1e9
            self.last_time = current_time

            

            # 使用 PID 控制器计算角速度
            angular_z = self.pid_controller.update(self.center[0], dt)
              
                
            # 根据 x 和 y 生成 Twist 消息
            twist_msg = Twist()
            twist_msg.linear.x = self.vx  # 示例：将 x 坐标转换为线速度
            twist_msg.angular.z = angular_z  # 使用 PID 控制器计算的角速度
            self.get_logger().info(f"{twist_msg.angular.z}")

            self.publisher.publish(twist_msg)

    #参数回调函数,回调使用        
    def parameters_callback(self,paramters):
        for paramter in paramters:
            self.get_logger().info("Parameter: {} - Value: {}".format(paramter.name, paramter.value))
            if paramter.name == "kp":
                self.kp = paramter.value
            elif paramter.name == "ki":
                self.ki = paramter.value   
            elif paramter.name == "kd":
                self.kd = paramter.value  
            elif paramter.name == "vx":
                self.vx = paramter.value
            elif paramter.name == "zhuitongyuzhi":
                self.zhuitongyuzhi = paramter.value   
            elif paramter.name == "qrcodeyuzhi":
                self.qrcodeyuzhi = paramter.value  
            elif paramter.name == "parkyuzhi":
                self.parkyuzhi = paramter.value
            elif paramter.name == "turn_time_1":
                self.turn_time_1 = paramter.value  
            elif paramter.name == "turn_time_2":
                self.turn_time_2 = paramter.value
            elif paramter.name == "turn_v":
                self.turn_v = paramter.value  
            elif paramter.name == "turn_w":
                self.turn_w = paramter.value 
            elif paramter.name == "turn_error":
                self.error = paramter.value          

        self.pid_controller = PIDController(Kp=self.kp,Ki=self.ki,Kd=self.kd,integral_max=100,setpoint=320)  # PID 控制器
        return SetParametersResult(successful=True)
def main(args=None):
    rclpy.init(args=args)
    coordinate_subscriber = CoordinateSubscriber()
    rclpy.spin(coordinate_subscriber)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

    """
    def action_cone(self):
        twist_msg = Twist()
        #self.state_obstacle = 1 
        self.obstacle_start_time = datetime.datetime.now() # 开始转弯并且计时
        twist_msg.linear.x = self.turn_v
        twist_msg.angular.z = -self.turn_w
        self.get_logger().info(f"{twist_msg.angular.z}")  
        self.publisher.publish(twist_msg)
        self.obstacle_now_time_1 = datetime.datetime.now()
        temp = (self.obstacle_now_time_1-self.obstacle_start_time)
        time = temp.microseconds/1000000 + temp.seconds
        if time > self.turn_time_1: # 时间到准备转回去，并开始计时
            self.obstacle_start_time = datetime.datetime.now()
            #self.state_obstacle = 2
            twist_msg.linear.x = self.turn_v
            twist_msg.angular.z = self.turn_w
            self.get_logger().info(f"{twist_msg.angular.z}")  
            self.publisher.publish(twist_msg)
        self.obstacle_now_time_2 = datetime.datetime.now()
        temp = (self.obstacle_now_time_2-self.obstacle_start_time)
        time = temp.microseconds/1000000 + temp.seconds
        if time > self.turn_time_2: # 时间到已经转回来了
            #self.state_obstacle = 0 
            twist_msg.linear.x = self.vx
            twist_msg.angular.z = 0.0
            self.get_logger().info(f"{twist_msg.linear.x}")  
            self.publisher.publish(twist_msg)
"""